#include <algorithm>
//#include <cmath>
#include <boost/units/cmath.hpp>
#include <iostream>
#include <math.h>

#include "segment.h"
#include "line.h"
#include "rayintersections.h"
#include "grid.h"



less_by_angle::less_by_angle(segment_t const & seg) : seg_(seg)
{
}

bool less_by_angle::operator() (segment_t const & l, segment_t const & r) const
{
    point_t point;
    intersects(l, r, point);

    point_t left;
    if(distance(point, l.first) < distance(point, l.second))
        left = l.second - point;
    else
        left = l.first - point;

    point_t right;
    if(distance(point, r.first) < distance(point, r.second))
        right = r.second - point;
    else
        right = r.first - point;

    if(seg_.first.x == seg_.second.x && seg_.first.y > seg_.second.y)
    {
        left = point_t(left.y, -left.x);
        right = point_t(right.y, -right.x);
    }
    else if(seg_.first.x == seg_.second.x && seg_.first.y < seg_.second.y)
    {
        left = point_t(-left.y, left.x);
        right = point_t(-right.y, right.x);
    }
    else if(seg_.first.x < seg_.second.x)
    {
        left.x = -left.x;
        right.x = -right.x;
    }

    double angle_l = atan2(left.y, left.x);
    double angle_r = atan2(right.y, right.x);

    if(angle_l < 0)
    {
        angle_l = -angle_l;
    }

    if(angle_r < 0)
    {
        angle_r = -angle_r;
    }

    return (angle_l < angle_r);
}


int segment_hash::operator()(segment_t const & segment) const
{
    return int(segment.first.x + segment.second.x);
}

point_pos point_position(segment_t const & segment, point_t const & point)
{
    if(segment.first == segment.second)
        return UNDEFINED;

    point_t a = segment.second - segment.first;
    point_t b = point - segment.first;
    double c_z = (a.x * b.y - a.y * b.x);
    if (c_z > 0)
        return LEFT;
    if (c_z == 0)
        return ON;
    return RIGHT;
}

bool intersects(segment_t const & l, segment_t const & r, point_t & res)
{
    line_t line_l(l.first, l.second);
    line_t line_r(r.first, r.second);
    
    bool value = intersects(line_l, line_r, res);
    if(value)
    {
        if(res.x <= std::max(l.first.x, l.second.x) + 0.000001 && res.x >= std::min(l.first.x, l.second.x) - 0.000001
                && res.y <= std::max(l.first.y, l.second.y) + 0.000001 && res.y >= std::min(l.first.y, l.second.y) - 0.000001)
        {
            if(res.x <= std::max(r.first.x, r.second.x) + 0.000001 && res.x >= std::min(r.first.x, r.second.x) - 0.000001
                    && res.y <= std::max(r.first.y, r.second.y) + 0.000001 && res.y >= std::min(r.first.y, r.second.y) - 0.000001)
            {
                return true;
            }
        }
    }
    return false;
}

bool intersects(segment_t const & l, segment_t const & r)
{
    point_t point;
    bool res = intersects(l, r, point);
    return res;
}

